#include "MultibodySystem.h"
#include "ConstraintUtils.h"
#include "Quaternion.h"
#include "List.h"

using namespace RSIM;

/////////////////////////////////////////////////////// 

void ConstraintUtils::updQForRigidBodyNode(const double *dq, 
                                           RigidBodyNode *Nd, 
                                           State *state,
                                           int& qid)const{
        double *Q = Nd->data_->wgetQ(*state);
        const int& nu = Nd->data_->getNDOF();        
        
        // Check if the node has quaternions in use.
        if(Nd->data_->areQuaternionsInUse()){                        
                {
                        // These are the actual increments in quaternions computed
                        // from the quasi-coordinates represented by dq
                        double dq_quat[4];
                        Quaternion::calcQDotFromAngularVelocity(dq+qid, // quasi-coordinates associated with angular velocity
                                                                Q,      // Current Euler parameters
                                                                dq_quat // dq_quat = 0.5*E'*w (see documentation in Quaternion.h
                                                                );                        
                        Q[0] += dq_quat[0];Q[1] += dq_quat[1];
                        Q[2] += dq_quat[2];Q[3] += dq_quat[3];                                                                
                }
                
                // Normalize the Quaternion
                Utils::normalize(4,Q);                

                // Used 3 delq's
                qid += 3;
                
                // This is in case the joint also has linear degrees of freedom.
                if(nu>3){
                        for(int j=0;j<nu-3;++j){
                                Q[4+j] += dq[qid++]; 
                        }
                }                        
        }//if(Nd->data_->areQuaternionsInUse())
        else{
                // If Quaternions are not in use, nu = nq
                // No need to normalize here.
                for(int j=0;j<nu;Q[j++] += dq[qid++]);                
        }
}

/////////////////////////////////////////////////////// 

void ConstraintUtils::updBranchQs(const double *dq,
                                  const List<int>& umap,
                                  const List<int>& path,
                                  MultibodySystem *MBS,
                                  State *state,
                                  int& qid)const{
        
        // update the q's associated with the parent branch
        for(int i=0;i<umap.getLength();++i){
                const int& uid = umap[i];
                
                // This condition implies that q has already been updated
                if(uid<qid)
                        continue;
                
                #ifdef RSIM_SAFE
                Utils::rsim_assert(qid == uid,
                                   "Bug in Constraint::updQ(). Check indexing.");
                #endif
                
                RigidBodyNode *Nd = MBS->wgetNode(path[i]);
                
                // This will increment qid by nu's associated with the node.
                ConstraintUtils::updQForRigidBodyNode(dq,Nd,state,qid);                
        }
}
                                 
/////////////////////////////////////////////////////// 